import java.util.ArrayList;
import java.util.Collection;

@SuppressWarnings("UnusedDeclaration")
public class KdTree {

    private class KdNode {
        private Point2D point;
        private KdNode left;
        private KdNode right;

        private KdNode(Point2D point, KdNode left, KdNode right) {
            this.point = point;
            this.left = left;
            this.right = right;
        }

        private KdNode(Point2D point) {
            this(point, null, null);
        }

        @Override
        public String toString() {
            return "KdNode{" +
                    "point=" + point +
                    '}';
        }
    }

    private KdNode root;
    private int size;

    // construct an empty set of points
    public KdTree() {

    }

    // is the set empty?
    public boolean isEmpty() {
        return 0 == size;
    }

    // number of points in the set
    public int size() {
        return size;
    }

    // add the point p to the set (if it is not already in the set)
    public void insert(Point2D p) {
        KdNode newNode = new KdNode(p);
        if (null == root) {
            root = newNode;
            size++;
        } else {
            KdNode parent = root;
            for (int level = 0; ; level++) {
                if (parent.point.equals(newNode.point)) {
                    break;
                }
                if ((0 == level % 2 && newNode.point.x() < parent.point.x()) || (1 == level % 2 && newNode.point.y() < parent.point.y())) {
                    if (null == parent.left) {
                        if (!parent.point.equals(newNode.point))  {
                            parent.left = newNode;
                            size++;
                        }
                        break;
                    } else {
                        parent = parent.left;
                    }
                } else {
                    if (null == parent.right) {
                        if (!parent.point.equals(newNode.point))  {
                            parent.right = newNode;
                            size++;
                        }
                        break;
                    } else {
                        parent = parent.right;
                    }
                }
            }
        }

    }

    // does the set contain the point p?
    public boolean contains(Point2D p) {
        KdNode parent = root;
        for (int level = 0; ; level++) {
            if (null == parent) {
                return false;
            }
            if (p.equals(parent.point)) {
                return true;
            } else {
                if ((0 == level % 2 && p.x() < parent.point.x()) || (1 == level % 2 && p.y() < parent.point.y())) {
                    parent = parent.left;
                } else {
                    parent = parent.right;
                }
            }
        }
    }

    private void draw(KdNode root) {
        if (root != null) {
            draw(root.left);
            root.point.draw();
            draw(root.right);
        }
    }

    // draw all of the points to standard draw
    public void draw() {
        draw(root);
    }

    private RectHV leftRect(KdNode node, RectHV parentRect, int level) {
        double xmin;
        double ymin;
        double xmax;
        double ymax;
        if (0 == level % 2) {
            xmin = parentRect.xmin();
            ymin = parentRect.ymin();
            xmax = node.point.x();
            ymax = parentRect.ymax();
        } else {
            xmin = parentRect.xmin();
            ymin = parentRect.ymin();
            xmax = parentRect.xmax();
            ymax = node.point.y();
        }
        return new RectHV(xmin, ymin, xmax, ymax);
    }

    private RectHV rightRect(KdNode node, RectHV parentRect, int level) {
        double xmin;
        double ymin;
        double xmax;
        double ymax;
        if (0 == level % 2) {
            xmin = node.point.x();
            ymin = parentRect.ymin();
            xmax = parentRect.xmax();
            ymax = parentRect.ymax();
        } else {
            xmin = parentRect.xmin();
            ymin = node.point.y();
            xmax = parentRect.xmax();
            ymax = parentRect.ymax();
        }
        return new RectHV(xmin, ymin, xmax, ymax);
    }

    private void range(RectHV rect, KdNode node, RectHV parentRect, int level, Collection<Point2D> result) {
        if (node != null) {
            if (rect.contains(node.point)) result.add(node.point);
            final RectHV leftRect = leftRect(node, parentRect, level);
            if (rect.intersects(leftRect)) {
                range(rect, node.left, leftRect, level+1, result);
            }
            final RectHV rightRect = rightRect(node, parentRect, level);
            if (rect.intersects(rightRect)) {
                range(rect, node.right, rightRect, level+1, result);
            }
        }
    }

    // all points in the set that are inside the rectangle
    public Iterable<Point2D> range(RectHV rect) {
        Collection<Point2D> result = new ArrayList<Point2D>();
        range(rect, root,
                new RectHV(-Double.MAX_VALUE, -Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE),
                0, result);
        return result;
    }

    private Point2D nearest(Point2D p, KdNode node, RectHV parentRect, int level, Point2D nearest) {
        if (node != null && parentRect.distanceSquaredTo(p) < nearest.distanceSquaredTo(p)) {
            if (node.point.distanceSquaredTo(p) < nearest.distanceSquaredTo(p)) nearest = node.point;
            Point2D nearestLeft = nearest(p, node.left, leftRect(node, parentRect, level), level+1, nearest);
            if (nearestLeft.distanceSquaredTo(p) < nearest.distanceSquaredTo(p)) nearest = nearestLeft;
            Point2D nearestRight = nearest(p, node.right, rightRect(node, parentRect, level), level+1, nearest);
            if (nearestRight.distanceSquaredTo(p) < nearest.distanceSquaredTo(p)) nearest = nearestRight;
        }
        return nearest;
    }

    // a nearest neighbor in the set to p; null if set is empty
    public Point2D nearest(Point2D p) {
        return nearest(p, root,
                new RectHV(-Double.MAX_VALUE, -Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE),
                0, new Point2D(Double.MAX_VALUE, Double.MAX_VALUE));
    }

}
